#include <dispatch_server/dispatch_server.h>
#include <string>
#include <libconfig.h++>
#include <chrono>
#include <thread>
#include <sch_common/sch_common_struct.hpp>
#include <pthread.h>
#include <dispatch_system/map_pretreatment.h>
#include <dispatch_server/deadlock_handler.h>

using namespace std;
using namespace moying::communication;
using namespace placeholders;
using namespace moying::sch;


//返回成功
#define RET_SUCCESS(res) \
    res.mutable_result()->set_code(ReqErrType::kReqSuccess); \
    res.mutable_result()->set_msg("SUCCESS");

//返回失败
#define RET_FAIL(res,code,msg) \
    res.mutable_result()->set_code(code); \
    res.mutable_result()->set_msg(msg);

//返回内部错误
#define RET_INTERNAL_FAIL(res) \
    res.mutable_result()->set_code(ReqErrType::kReqInternalErr); \
    res.mutable_result()->set_msg("request failed!");

#define RESPONSE(ret) \
    if(ReqErrType::kReqSuccess == ret){ \
        res.mutable_result()->set_code(ReqErrType::kReqSuccess); \
        res.mutable_result()->set_msg("SUCCESS"); \
    }else{ \
        res.mutable_result()->set_code(ret); \
        res.mutable_result()->set_msg("request failed!"); \
    }

namespace moying{

namespace communication
{

DispatchServerPtr DispatchServer::ptr_ = nullptr;
std::mutex DispatchServer::singleton_mutex_;

DispatchServerPtr DispatchServer::instance(){
    if(ptr_ != nullptr)
        return ptr_;
    
    singleton_mutex_.lock();
    if(ptr_ != nullptr){
        singleton_mutex_.unlock();
        return ptr_;
    }
    ptr_ = std::make_shared<DispatchServer>();
    singleton_mutex_.unlock();
    return ptr_;
}

bool DispatchServer::initialize(std::string ws,std::string map)
{
    {
        std::lock_guard<std::mutex> lock(time_map_mutex_);
        moyingmap_  = std::make_shared<navigation::MoyingMap>();
    }
    
    astar_      = std::make_shared<navigation::TimeBasedAStar>();

    std::string cfg_path = "./configs/schedule.cfg";
    LOG_INFO("schedule config file path:"<<cfg_path);
    bool ret = ScheduleConfig::instance()->run(cfg_path);
    if(!ret){
        LOG_ERROR("schedule config start failed!");
        return false;
    }else{
        LOG_INFO("schedule config start success");
    }
    ConfigInfo& cfg_info = ScheduleConfig::instance()->getConfigInfo();

    LOG_INFO("=== robot size count:"<<cfg_info.robot_size_cfgs.size());
    ws = ws.empty() ? cfg_info.map_cfg.cur_workspace : ws;
    map = map.empty() ? cfg_info.map_cfg.cur_map : map;
    ret = initPathPlan(ws,map);
    if(ret)
        startPlanningThread();
    return ret;

}

bool DispatchServer::initPathPlan(std::string ws,std::string map){

    double resolution;
    std::vector<double> origin;
    bool negate = false;
    double free_thresh;
    double occupied_thresh;

    std::string map_folder = "./maps/" + ws + "/" + map;
    std::string map_file = map_folder + "/" + map + ".png";
    std::string map_config =  map_folder + "/" + map + ".cfg";

    ConfigInfo& cfg_info = ScheduleConfig::instance()->getConfigInfo();

    navigation::CostMapPtr costmap = std::make_shared<navigation::CostMap>();
    auto traffic_cfg = cfg_info.traffic_manager_cfg;
    costmap->setMapOffset(traffic_cfg.map_offset_x,traffic_cfg.map_offset_y);
    try
    {
        LOG_INFO("map cfg path:"<<map_config);
        libconfig::Config cfg;
        cfg.readFile(map_config.c_str());
        libconfig::Setting& root = cfg.getRoot();
        cfg.lookupValue("resolution", resolution);
        cfg.lookupValue("occupied_thresh", occupied_thresh);
        cfg.lookupValue("free_thresh", free_thresh);
        libconfig::Setting& ori = root["origin"];

        for(int i=0; i < ori.getLength(); ++i)
        {
            origin.push_back(static_cast<double>(ori[i]));
        }
        origin[1] += 0.05;

        if(!costmap->loadStaticMapFromFile(map_file, resolution, negate, occupied_thresh, free_thresh, origin))
        {
            ERROR("Load costmap failed");
            return false;
        }
    }
    catch(const std::exception& e)
    {
        std::cerr << "read map cfg file failed,"<<e.what() << '\n';
        return false;
    }
    
    
    double inflate = cfg_info.traffic_manager_cfg.static_map_inflate;

    WARNING("begin inflate static map:"<<time(0));
    costmap->inflateStaticMap(inflate);
    WARNING("end inflate static map:"<<time(0));

    WARNING("begin setup planning cost map:"<<time(0));
    costmap->setupPlanningCostMap(cfg_info.traffic_manager_cfg.path_plan_map_resolution);
    WARNING("end setup planning cost map:"<<time(0));

    bool ret = moyingmap_->init(costmap);
    if(ret){
        moyingmap_->set_init_success(true);
        LOG_INFO("moyingmap init success!");
    }else{
        moyingmap_->set_init_success(false);
        LOG_ERROR("moyingmap init failed!");
    }
    
    

    ret = astar_->init(moyingmap_);
    if(ret){
        LOG_INFO("astar_ init success!");
    }else{
        LOG_ERROR("astar_ init failed!");
    }

    map_viz_ = std::make_shared<visualization::MoyingMapViz>(moyingmap_);
    map_viz_->publishCostmapCoordinate();
    map_viz_->publishMapCell(cfg_info.robot_size_cfgs[0].length,cfg_info.robot_size_cfgs[0].width);

    MapDimension d = moyingmap_->getDimensions();
    HIGHLIGHT("Dimension of moying map is x:"<<d.x<<" y:"<<d.y<<" theta:"<<d.theta<<" time:"<<d.time);

#if defined(__DEBUG__)
    //发布更改过导航坐标cell的rviz显示
    
    auto ccs = moyingmap_->corrected_cells(cfg_info.robot_size_cfgs[0].length,cfg_info.robot_size_cfgs[0].width);
    if(ccs != nullptr){
        std::vector<CorrectCellInfo> cci_vec;
        for(int x=0;x<d.x;x++){
            for(int y=0;y<d.y;y++){
                
                auto cci = (*ccs)[x][y];
                if(cci.cell_x != -1){
                    cci_vec.emplace_back(cci);
                }
            }
        }
        float len = 1+cfg_info.traffic_manager_cfg.robot_length_padding;
        float width = 0.618+cfg_info.traffic_manager_cfg.robot_width_padding;
        map_viz_->publishCorrectedCell(cci_vec,len,width);
    }
    
#endif

    if(deadlock_handler_ != nullptr)
        delete deadlock_handler_;
    deadlock_handler_ = new DeadLockHandler(moyingmap_);

    LOG_INFO("DispatchServer is ready");

    // switch_map_req_.set_workspace("");
    // switch_map_req_.set_map_name("");
    return true;
}

bool DispatchServer::packLogFunc(const std::string& msg){
    // LOG_INFO(msg);
    HIGHLIGHT(msg);
}


bool DispatchServer::run(){
    const ConfigInfo& cfg_info = ScheduleConfig::instance()->getConfigInfo();
    loadConstNavPathData();


    replier_ = std::make_shared<MoyingCommuReply>("*", cfg_info.commu_cfg.traffic_manage_req_port, "tm_rep");

    replier_->setPollTimeout(5000);                  //optional, set poll timeout, 1 seconds default
    replier_->setHeartbeatIVL(1000);                    //optional, set heartbeat interval, hasn't heartbeat checking default
    replier_->setHeartbeatTimeout(5000);                 
    replier_->setPrintPackCall(std::bind(&DispatchServer::packLogFunc,this,placeholders::_1));
    replier_->run();                                 //essential, start up communication threads
    
    replier_->advertiseService(tm_request_ids[0],&DispatchServer::handlePathPlanService,this);
    replier_->advertiseService(tm_request_ids[1],&DispatchServer::handleCancelNavPathService,this);
    replier_->advertiseService(tm_request_ids[2],&DispatchServer::handleAllPathPlanService,this,false);
    replier_->advertiseService(tm_request_ids[3],&DispatchServer::handleSwitchMapTMService,this);
    replier_->advertiseService(tm_request_ids[4],&DispatchServer::handleOccupyTimeMapService,this);
    replier_->advertiseService(tm_request_ids[5],&DispatchServer::handleWhetherPoseInOneWayRoadService,this);
    replier_->advertiseService(tm_request_ids[6],&DispatchServer::handleManageConstNavPathService,this);

    publisher_ = std::make_shared<MoyingCommuPublish>("*", cfg_info.commu_cfg.traffic_manage_sub_port, "tm_pub");
    publisher_->setPollTimeout(5000);
    publisher_->setPrintPackCall(std::bind(&DispatchServer::packLogFunc,this,placeholders::_1));
    publisher_->run();

    SystemManagerCommuClient::instance()->setRecvRobotListCallback([](const std::map<IdType, RobotPtr>& all_robots )
    {
        //all_robots里存储了初始的机器人列表  need to do;
        HIGHLIGHT("Find "<<all_robots.size()<<" robot"<<(all_robots.size()>1?"s":""));
        if(ptr_ == nullptr)
            return;
        unsigned int map_x, map_y;
        for(auto robot : all_robots)
        {
            ptr_->addVehicle(robot.second);
        }
    });

    SystemManagerCommuClient::instance()->subRobotOnOffLineNotify(std::bind(&DispatchServer::robotOnoffLineNotifyCB,this,std::placeholders::_1));
    SystemManagerCommuClient::instance()->subRobotPoseNotify(std::bind(&DispatchServer::robotPoseNotifyCB,this,std::placeholders::_1));
    
    SystemManagerCommuClient::instance()->connectToSystemManager(cfg_info.commu_cfg.system_manage_ip,
                cfg_info.commu_cfg.system_manage_req_port,cfg_info.commu_cfg.system_manage_sub_port,false,"traffic_manager");
    
    subSystemManageNotifies();
    return true;
}


void DispatchServer::subSystemManageNotifies(){

    SystemManagerCommuClient::instance()->subRobotNavStateNotify(std::bind(&DispatchServer::robotNavStateNotifyCB,this,std::placeholders::_1));
    // SystemManagerCommuClient::instance()->subRobotLocalCostMapNotify(std::bind(&DispatchServer::localCostmapNotifyCB,this,std::placeholders::_1));
    SystemManagerCommuClient::instance()->subRobotLocalObstacleNotify(std::bind(&DispatchServer::localObstacleNotifyCB,this,std::placeholders::_1));
    SystemManagerCommuClient::instance()->subNotify<RobotSizeUpdateNotify>(system_manager_notify_ids[16],
            std::bind(&DispatchServer::robotSizeUpdateNotify,this,std::placeholders::_1));
    SystemManagerCommuClient::instance()->subNotify<MapFileDataUpdateNotify>(system_manager_notify_ids[13],
            std::bind(&DispatchServer::mapFileDataUpdateNotify,this,std::placeholders::_1));
    
}

void DispatchServer::robotPoseNotifyCB (const moying_proto::RobotPoseNotify& notify){

    if(!moyingmap_->init_success())
        return;
    IdType rob_id = notify.robot_id();
    navigation::VehiclePtr vehicle = moyingmap_->getVehicle(rob_id);
    if(vehicle != nullptr)
    {
        moyingmap_->updateVehiclePose(rob_id, notify.pose().x(), notify.pose().y(), notify.pose().theta());
#if defined(__DEBUG__)
        std::lock_guard<std::recursive_mutex> lock(vehicle_mutex_);
        if(vehicle_viz_[rob_id])
            vehicle_viz_[rob_id]->publishRobotOccupancy();
#endif
        // HIGHLIGHT("Update robot state "<<notify.pos().x()<<" "<<notify.pos().x());
    }
    else
    {
        WARNING("Unknow robot ID "<<rob_id);
    }

}

void DispatchServer::robotOnoffLineNotifyCB (const moying_proto::RobotOnOffLineNotify& notify){


    int type = notify.on_off_line();
    if(type == 0){
        // 添加机器人
        if(addVehicle(notify.info().id(), notify.info().type(), notify.info().nav_base_type(),notify.info().name(), 
                        notify.info().pose().x(), notify.info().pose().y(), notify.info().pose().theta()))
        {
            HIGHLIGHT("robot online -- robot id:" << notify.info().id());
        }
        else
        {
            WARNING("robot online failed -- robot id:" << notify.info().id());
        }
        
    }
    else if(type == 1){
        //删除机器人时机
        HIGHLIGHT("robot offline -- robot id:" << notify.robot_id());
        deleteVehicle(notify.robot_id());
    }

}

void DispatchServer::robotNavStateNotifyCB (const moying_proto::NavStateNotify& notify){
    NavStateInfo state_info = (NavStateInfo)notify.state_info();
    IdType robot_id = notify.robot_id();

    if(!moyingmap_->init_success())
        return;

    auto vehicle = moyingmap_->getVehicle(robot_id);

    unsigned long long arrived_time = state_info.timestamp();
    
    unsigned int reached_index = notify.state_info().path_index();
    // HIGHLIGHT("Got nav state "<<state_info.state());
    bool nav_path_clear = false;
    switch (state_info.state())
    {
        case NavState::kArriveStepDestPos:
        {
            
        }
        break;

        case NavState::kArriveFinalDestPos:
        {
            // std::lock_guard<std::recursive_mutex> lock(vehicle_mutex_);

            // moyingmap_->clearVehiclePath(robot_id);
            // nav_path_clear = true;

            HIGHLIGHT("arrive dest pose successfully, robot_id:"<<robot_id<<
                " x:"<<state_info.pose().x()<<" y:"<<state_info.pose().y()<<" th:"<<state_info.pose().theta());

            vehicle->path_plan_result().clear();
        }
            break;
        case NavState::kNavCanceled:
        case NavState::kNavFailed:
            // nav_path_clear = true;
        default:
            WARNING("nav state notify ,state:"<<state_info.state()<<" robot_id:"<<robot_id<<
                " x:"<<state_info.pose().x()<<" y:"<<state_info.pose().y()<<" th:"<<state_info.pose().theta());
            vehicle->path_plan_result().clear();
            break;
    }

    if(vehicle != nullptr){
        if( vehicle->path_plan_result().nav_type == NavType::kNavTypeConstantPath
         || vehicle->path_plan_result().path.empty()){ 
            return;
        }
    }

    {
        std::lock_guard<std::recursive_mutex> lock(vehicle_mutex_);
        auto iter = vehicle_monitors_.find(robot_id);
        if( iter != vehicle_monitors_.end())
            iter->second->notifyArrived(notify); 
    }

    if(deadlock_handler_){
        deadlock_handler_->handleNavStateNotify(notify);
    }

}


void DispatchServer::localObstacleNotifyCB(const moying_proto::LocalObstacleNotify& notify)
{
    if(!moyingmap_->init_success())
        return;

    unsigned int robot_id = notify.robot_id();
    std::lock_guard<std::recursive_mutex> lock(vehicle_mutex_);

    auto iter = vehicle_monitors_.find(robot_id);
    if( iter != vehicle_monitors_.end()){
        iter->second->localObstacleCallback(notify.obstacle());
    }else
        WARNING("Got local obstacle from unknown robot "<<robot_id);
}

void DispatchServer::robotSizeUpdateNotify(const moying_proto::RobotSizeUpdateNotify& notify){

    HIGHLIGHT("recv reset robot_size,len:"<<notify.size_3d().length()<<" width:"<<notify.size_3d().width()<<" robot_id:"<<notify.robot_id());
    if(!moyingmap_->init_success())
        return;

    if(notify.robot_id() <= 0 || moyingmap_ == nullptr)
        return;
    moyingmap_->ergodicRobots([notify](VehiclePtr vehicle)->bool{
        if(vehicle && vehicle->getID() == notify.robot_id()){
            auto size = notify.size_3d();
            vehicle->setRobotSize(size.length(),size.width(),size.height());
            return false;
        }
        return true;
    });
}

void DispatchServer::mapFileDataUpdateNotify(const moying_proto::MapFileDataUpdateNotify& notify){

    if(!moyingmap_->init_success())
        return;

    std::string ws = notify.workspace();
    std::string map = notify.map();
    auto cfg = ScheduleConfig::instance()->getConfigInfo();
    if(cfg.map_cfg.cur_workspace.compare(ws) != 0 || cfg.map_cfg.cur_map.compare(map) != 0)
        return;

    bool no_robot_recv_path_plan = true;
    moyingmap_->ergodicRobots([&no_robot_recv_path_plan](VehiclePtr vehicle)->bool{
        if(vehicle->recv_path_plan_req()){
            no_robot_recv_path_plan = false;
            return false;
        }
        return true;
    });

    if(!no_robot_recv_path_plan){
        LOG_WARN("Can't switch map,there are robots to path planning");
        return;
    }

    is_updating_map_data_ = true;
    std::thread  switch_map_thread([this](void){
        bool ret = initialize();
        if(ret){
            SystemManagerCommuClient::instance()->disconnectWithSystemManager();

            {
                std::lock_guard<std::recursive_mutex> lock(vehicle_mutex_);
                vehicle_viz_.clear();
                vehicle_monitors_.clear();
            }

            SystemManagerCommuClient::instance()->subRobotOnOffLineNotify(std::bind(&DispatchServer::robotOnoffLineNotifyCB,this,std::placeholders::_1));
            SystemManagerCommuClient::instance()->subRobotPoseNotify(std::bind(&DispatchServer::robotPoseNotifyCB,this,std::placeholders::_1));

            const ConfigInfo& cfg_info = ScheduleConfig::instance()->getConfigInfo();
            ret = SystemManagerCommuClient::instance()->connectToSystemManager(cfg_info.commu_cfg.system_manage_ip,
                cfg_info.commu_cfg.system_manage_req_port,cfg_info.commu_cfg.system_manage_sub_port,false,"traffic_manager");
            if(ret){
                subSystemManageNotifies();
                LOG_INFO("reconnect system_manager success");
            }else{
                LOG_INFO("reconnect system_manager failed");

            }
        }
        is_updating_map_data_ = false;
    });
    switch_map_thread.detach();

}

void DispatchServer::startPlanningThread()
{
    HIGHLIGHT("wait last path plan thread stop...");
    while(path_plan_thread_is_running_){    //等待之前线程退出
        stop_path_plan_thread_ = true;
        this_thread::sleep_for(std::chrono::milliseconds(100));
    }
    HIGHLIGHT("last path plan thread stoped!");
    stop_path_plan_thread_ = false;

    start_time_ = std::chrono::system_clock::now();
    sec_since_start_ = 0;

    auto planning_thread = [&,this]() -> void {

        this->path_plan_thread_is_running_ = true;

        auto schedule_cfg = sch::ScheduleConfig::instance()->getConfigInfo();
        bool deadlock_handle = schedule_cfg.traffic_manager_cfg.deadlock_handle;

        float path_plan_map_res = schedule_cfg.traffic_manager_cfg.path_plan_map_resolution;
        float robot_nav_speed = schedule_cfg.traffic_manager_cfg.robot_nav_speed;
        robot_nav_speed = (robot_nav_speed <= 0 || robot_nav_speed >= 3) ? 0.5 : robot_nav_speed;
        path_plan_map_res = (path_plan_map_res <= 0 || path_plan_map_res > 1) ? 0.5 : path_plan_map_res;

        float time_res = 1/(robot_nav_speed/path_plan_map_res);

        next_update_time_ = std::chrono::system_clock::now();
        while(true)
        {
            if(stop_path_plan_thread_){
                path_plan_thread_is_running_ = false;
                return;
            }else{
                // printf("loop\n");
            }
            if(is_updating_map_data_){
                    std::this_thread::sleep_for(std::chrono::milliseconds(10));
                    continue;
            }

            auto now = std::chrono::system_clock::now();
            int update_times = std::chrono::duration_cast<std::chrono::milliseconds>(now - next_update_time_).count()/1000;
            update_times = update_times < 0 ? 1 : update_times+1;
            if(update_times > 1){
                WARNING("time map update delayed, will update "<<update_times<<" times once!");
            }

            current_update_time_ = now;
            // next_update_time_ = current_update_time_ + std::chrono::milliseconds(1000);
            next_update_time_ = current_update_time_ + std::chrono::milliseconds(int(time_res*1000));
            // auto t = std::chrono::system_clock::now();
            if( !is_updating_map_data_ ){
                std::lock_guard<std::mutex> lock(time_map_mutex_);
                moyingmap_->updateTimeMap(current_update_time_,update_times);
            }
            
            // HIGHLIGHT("eclapse:"<<std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now() - t).count());
            while(std::chrono::duration_cast<std::chrono::milliseconds>(next_update_time_ - std::chrono::system_clock::now()).count() > 10)
            {
                if(planning_queue_.size() == 0)
                {
                    std::this_thread::sleep_for(std::chrono::milliseconds(10));
                    if(deadlock_handle){
                        deadlock_handler_->handleDeadLock();
                    }
                    continue;
                }

                //切换地图时不执行路径规划
                if(is_updating_map_data_)
                    break;
                // if(!switch_map_req_.workspace().empty() && !switch_map_req_.map_name().empty()){
                //     break;
                // }

                unsigned int robot_id = 0;
                navigation::PlanningResult result;
                PathPlanningRequest request;
                navigation::VehiclePtr vehicle = nullptr;
                navigation::VehicleState start_state, goal_state;

                //从队列取出规划请求
                {
                    std::lock_guard<std::mutex> lock(planning_queue_mutex_);
                    for(auto iter= planning_queue_.begin();iter!=planning_queue_.end();iter++){
                        
                        robot_id = iter->first;
                        request = iter->second;
                        if( request.path_plan_start_time == 0 || (request.path_plan_start_time != 0 && time(0) >= request.path_plan_start_time) ){
                            
                            if(request.path_plan_start_time != 0){
                                request.start = request.vehicle->getPose();
                            }
                            vehicle = request.vehicle;
                            if(request.is_global_goal){
                                vehicle->set_global_goal(request.goal);
                                SearchCellArea area;
                                area.valid = false;
                                astar_->set_search_cell_area(area);
                            }else{
                                astar_->set_search_cell_area(request.search_area);
                            }
                            start_state = request.start;
                            goal_state = request.goal;
                            result.start_time = current_update_time_;
                            start_state.t = result.start_time;
                            planning_queue_.erase(iter);
                            break;
                        }else{
                            continue;
                        }

                        
                    }
                    
                    if(vehicle == nullptr){
                        std::this_thread::sleep_for(std::chrono::milliseconds(1));
                        continue;
                    }

                }
                
                if(deadlock_handle && request.is_query_from_outside &&  deadlock_handler_->isRobotInDeadLockGroup(vehicle)){   //如果是机器人在处理死锁中,不接受外部发送的规划请求
                        continue;
                }
                
                bool path_plan_result = false;
                if(astar_->searchPath(vehicle, start_state, goal_state, result))
                {
                    std::lock_guard<std::recursive_mutex> lock(vehicle_mutex_);
                    bool can_enter_oneway_road = true;
                    auto iter = vehicle_monitors_.find(robot_id);
                    if( iter != vehicle_monitors_.end()){
                        if( !vehicle_monitors_[robot_id]->checkOneWayRoadCanPass(result) ){
                            can_enter_oneway_road = false;
                            vehicle->set_last_path_plan_result(6);  //设置单向道不可通过
                        }
                    }

                    if( can_enter_oneway_road ){
                        if( request.is_revise_path ){
                            auto dest_pose = vehicle->path_plan_result().path.back();
                            if( result.path.back().t + std::chrono::seconds(3) > dest_pose.t){   
                                INFO("new path is not better,don't use");
                                vehicle->set_recv_path_plan_req(false);
                                continue;//修正后的路径不比原来路径更早到达,不使用新路径
                            }else{
                                INFO("use revise path");
                            }
                        }
                        result.occupy_time_after_arrive = request.occupy_time_after_arrive;
                        path_plan_result = true;
                        moyingmap_->clearVehiclePath(vehicle);
                        std::vector<PathPoint> path;
                        getProtoPath(result,path);
                        notifyPathPlanData(path, robot_id);
                        // notifyFollowingPath(path,robot_id);
                        std::lock_guard<std::recursive_mutex> lock(vehicle_mutex_);
    #if defined(__DEBUG__)
                        showPathPoint(robot_id,result, path);
                        if(vehicle_viz_[robot_id]){
                            vehicle_viz_[robot_id]->publishTrajectory(result);
                            vehicle_viz_[robot_id]->publishTimeFootprint(result);
                        }
    #endif
                        auto iter = vehicle_monitors_.find(robot_id);
                        if( iter != vehicle_monitors_.end()){
                            iter->second->startMonitoring(result);
                        }
                        result.nav_type = kNavTypeTrafficManager;
                        updateVehiclePathResult(robot_id,result);
                    }
                    
                }else{
                    if( request.is_revise_path ){
                        vehicle->set_recv_path_plan_req(false);
                        continue;
                    }
                    //如果路径规划失败,并且离障碍物太近,先脱离开障碍物
                    std::lock_guard<std::recursive_mutex> lock(vehicle_mutex_);
                    auto iter = vehicle_monitors_.find(robot_id);
                    if( iter != vehicle_monitors_.end()){
                        vehicle_monitors_[robot_id]->clearTimemapFootprint();
                        float min_dis = 10000;
                        moyingmap_->getRobotMinDisToObstacle(vehicle->getID(),min_dis);
                        if( (vehicle->path_plan_fail_times() > 5 || vehicle->last_path_plan_result() == 4) 
                            &&  vehicle->last_path_plan_result() != 2
                            ){
                                if(vehicle->path_plan_fail_times() > 10){   //长时间连续规划失败,脱离一下障碍物
                                    iter->second->tryEmergencyRecovery();
                                    vehicle->set_path_plan_fail_times(0);
                                }else if(min_dis < MIN_DIS_OBSTACLE  ){
                                    iter->second->tryEmergencyRecovery();
                                    // vehicle->set_path_plan_fail_times(0);
                                }else{
                                    auto pose =vehicle->getPose();
                                    if( moyingmap_->checkSquareEdgeIsObstacle(pose.x,pose.y,vehicle->getLength()+0.6) ){
                                        iter->second->tryEmergencyRecovery();
                                    }
                                }
                        }
                        HIGHLIGHT("path plan failed,min obstacle dis:"<<min_dis<<" robot_id:"<<vehicle->getID()
                            <<" last_path_plan_result:"<<vehicle->last_path_plan_result());
                    }
                    vehicle->path_plan_result().clear();
 
                }

                //重置收到路径规划请求标志
                if(path_plan_result){
                    vehicle->set_path_plan_fail_times(0);
                }else{
                    int times = vehicle->path_plan_fail_times()+1;
                    vehicle->set_path_plan_fail_times(times);
                } 
                vehicle->set_recv_path_plan_req(false);

                if(deadlock_handle){
                    //处理机器人死锁的情况
                    deadlock_handler_->updatePathPlanResult(vehicle,path_plan_result,request.goal);
                    deadlock_handler_->handleDeadLock();
                }
                
                //只有全局路径的请求,且没有发生死锁时才通知规划结果
                if( !deadlock_handle || (!deadlock_handler_->isRobotInDeadLockGroup(vehicle) && request.is_global_goal) ){
                    notifyPlanResult(robot_id, path_plan_result?1:0);
                }
                
                
#if defined(__DEBUG__)
                Local3DMap local_map = astar_->get_local_map();
                if(vehicle_viz_[robot_id]){
                    vehicle_viz_[robot_id]->publishAStarSearchNodes(astar_->get_closed(false),true,false,local_map.min_cell_x,local_map.min_cell_y,local_map.min_time);
                    vehicle_viz_[robot_id]->publishCollidePolygon(astar_->get_collide_pt_vector());
                    auto lcm = astar_->local_cost_map();
                    vehicle_viz_[robot_id]->publishLocalCostMap(lcm->local_cost_map(),lcm->min_cell_x(),lcm->min_cell_y());
                }
#endif
            }

            //路径监控模块检测前方障碍物
            vehicle_mutex_.lock();
            std::map<unsigned int, navigation::VehicleMonitorPtr> temp_monitors = vehicle_monitors_;
            vehicle_mutex_.unlock();
            for(auto monitor : temp_monitors){
                if(nullptr == monitor.second)
                    continue;
                monitor.second->check();
            } 

            //机器人没有路径时,占用一定时间的时间地图
            // DispatchServer::instance()->moyingmap()->ergodicRobots( [](VehiclePtr vehicle)->bool{
            //     if(vehicle == nullptr)
            //         return true;
            //     int occupy_seconds = 40;
            //     DispatchServer::instance()->noPathVehicleOccupyTimemap(vehicle->getID(),occupy_seconds);
            //     return true;
                
            // } );

            // map_viz_->publishTimedMap(60);

            std::this_thread::sleep_until(next_update_time_);
            sec_since_start_++;
        }
        path_plan_thread_is_running_ = false;
    };

    std::thread thr(planning_thread);
    thr.detach();

#if defined(__DEBUG__)
    auto pubVizThread = [=](){
        while(1){
            std::this_thread::sleep_for(std::chrono::milliseconds(1000));
            map_viz_->publishTimedMap(time_map_mutex_,60);
        }
    };
    std::thread pub_viz_thread(pubVizThread);
    pub_viz_thread.detach();
#endif
}

bool DispatchServer::noPathVehicleOccupyTimemap(unsigned int vehicle_id,int seconds){

    // printf("enter noPathVehicleOccupyTimemap\n");
    //对于没有路径的机器人,占用当前时刻到之后一段时间内的时间地图,防止别的机器人从身上规划路径

    auto robot_ptr = SystemManagerCommuClient::instance()->getRobotById(vehicle_id);
    if(robot_ptr == nullptr)
        return false;

    //在停车位或者充电桩时不占用时间地图
    if(!robot_ptr->park_station_id().empty() || !robot_ptr->charge_station_id().empty()){
        return false;
    }
    // std::lock_guard<std::recursive_mutex> lock(vehicle_mutex_);
    auto iter = vehicle_monitors_.find(vehicle_id);
    if( iter != vehicle_monitors_.end() && !iter->second->isTracking()){
        // printf("enter occupyTimeMap\n");
        DispatchServer::instance()->moyingmap()->occupyTimeMap(vehicle_id,seconds);
        return true;
    }else{
        return false;
    }
}

void DispatchServer::handleSendStopToRobot(IdType robot_id){

    if(deadlock_handler_){
        deadlock_handler_->handleRobotStopNav(robot_id);
    }
}

bool DispatchServer::addVehicle(unsigned int id, std::string type,moying_proto::NavBaseType base_type, const std::string &name, double x, double y, double theta)
{
    if(!moyingmap_->init_success())
        return false;
    if(moyingmap_->addVehicle(id, type, name,base_type))
    {
        sch::ConfigInfo cfg_info = ScheduleConfig::instance()->getConfigInfo();
        for(auto robot_size : cfg_info.robot_size_cfgs){
            if(robot_size.robot_type.compare(type) == 0){
               VehiclePtr vehicle_ptr = moyingmap_->getVehicle(id);
               if(vehicle_ptr != nullptr){
                vehicle_ptr->setRobotSize(robot_size.length,robot_size.width,robot_size.height);
                INFO("robot size is length:"<<robot_size.length<<" widht:"<<robot_size.width<<" height:"<<robot_size.height
                        <<" id:"<<id<<" robot_type:"<<robot_size.robot_type);
               }
               break;
            }
        }

        moyingmap_->updateVehiclePose(id, x, y, theta);
        // moyingmap_->registerStaticRobotOccupation(id);
        {
            std::lock_guard<std::recursive_mutex> lock(vehicle_mutex_);
#if defined(__DEBUG__)
            vehicle_viz_.emplace(id, std::make_shared<visualization::VehicleViz>(moyingmap_->getVehicle(id), moyingmap_));
#endif
            vehicle_monitors_.emplace(id, std::make_shared<navigation::VehicleMonitor>(moyingmap_, moyingmap_->getVehicle(id), vehicle_viz_[id], publisher_, tm_notify_ids[0]));

        }

        // bool r = moyingmap_->isCellCanRotate(8,10,2,6,moyingmap_->getVehicle(id));
        // printf("==== isCellCanRotate 8 10 2 6 result:%d\n",(int)r);
        return true;
    }
    else
    {
        HIGHLIGHT("Add robot return false");
        return false;
    }
    
}

bool DispatchServer::addVehicle(const RobotPtr &robot)
{
    unsigned int id = static_cast<unsigned int>(robot->id());
    std::string type = robot->type();
    std::string name = robot->name();
    double x = static_cast<double>(robot->pose().x());
    double y = static_cast<double>(robot->pose().y());
    double theta = static_cast<double>(robot->pose().theta());
    return addVehicle(id, type,robot->nav_base_type(), name, x, y, theta);
}

bool DispatchServer::deleteVehicle(unsigned int id)
{
    if(!moyingmap_->init_success())
        return false;

    navigation::VehiclePtr vehicle = moyingmap_->getVehicle(id);
    if(vehicle)
    {
        // {
            // std::lock_guard<std::recursive_mutex> lock(vehicle_mutex_);
            // moyingmap_->clearVehiclePath(vehicle, vehicle_paths_[id].first);
            // vehicle_paths_.erase(id);
        // }
        
        moyingmap_->clearVehiclePath(vehicle);
        {
            std::lock_guard<std::recursive_mutex> lock(vehicle_mutex_);
#if defined(__DEBUG__)
            vehicle_viz_.erase(id);
#endif
            if(vehicle_monitors_.find(id) != vehicle_monitors_.end()){
                vehicle_monitors_[id]->stopRobot(0);
                vehicle_monitors_.erase(id);
            }
            
        }
        return moyingmap_->deleteVehicle(id);
    }
    return false;
}

void DispatchServer::stateToPathPoint(const VehicleState &state, PathPoint &point,int point_index,int point_property)
{

    point.set_vx(state.vel_x);
    point.set_vy(state.vel_y);
    point.set_vth(state.vel_th);
    point.mutable_pose()->set_x(state.x);
    point.mutable_pose()->set_y(state.y);
    point.mutable_pose()->set_theta(state.th);
    point.set_timestamp(std::chrono::duration_cast<std::chrono::milliseconds>(state.t.time_since_epoch()).count());
    point.set_pt_index(point_index);
    point.set_property(point_property);
    point.set_is_last_pos(point_property == 2);

}

void DispatchServer::navConstantPath(const moying_proto::PathPlanService_Request& req, moying_proto::PathPlanService_Response& res){

    IdType robot_id = req.robot_id();
    navigation::VehiclePtr vehicle = moyingmap_->getVehicle(robot_id);

    string src_id = req.src_constant_path_point_id();   //固定路径起点
    string dest_id = req.dest_constant_path_point_id(); //固定路径终点

    ConstantPathInfo    cpi;

    //TODO:选择路径, 路径数组的操作要做多线程同步处理
    {

        bool searched = false;
        std::lock_guard<std::recursive_mutex> lock(data_mutex_);
        for(auto constant_path :all_const_nav_path_.all_path){
            if(!req.constant_path_id().empty() ){
                if(req.constant_path_id().compare(constant_path.path_id()) == 0){
                    cpi = constant_path;
                    cpi.set_reverse(req.reverse());
                    searched = true;
                    break;
                }
            }else if(constant_path.src_id().compare(src_id) == 0 && constant_path.dest_id().compare(dest_id) == 0){
                cpi = constant_path;
                cpi.set_reverse(false);
                cpi.set_backwards(false);
                cpi.set_continuous_trajectory(true);
                searched = true;
                break;
            }else if(constant_path.src_id().compare(dest_id) == 0 && constant_path.dest_id().compare(src_id) == 0){
                cpi = constant_path;
                cpi.set_reverse(true);
                cpi.set_backwards(false);
                cpi.set_continuous_trajectory(true);
                searched = true;
                break;
            }
        }
        if(src_id == dest_id && !searched){
            cpi.set_map_name(req.map_name());
            cpi.set_src_id(src_id);
            cpi.set_dest_id(dest_id);
            ConstantPathPoint pt;
            *pt.mutable_pose() = req.dest_pt();
            *cpi.mutable_path_point()->Add() = pt;
            *cpi.mutable_path_point()->Add() = pt;
            cpi.set_reverse(false);
            cpi.set_backwards(false);
            cpi.set_continuous_trajectory(true);
            searched = true;
        }
        if(!searched){
            RET_FAIL(res,kReqStateNotSatisfied,"未找到合适的固定路径");
            return;
        }
    }

    //设置结果
    PlanningResult result;
    result.cpi              = cpi;
    result.nav_type         = kNavTypeConstantPath;
    updateVehiclePathResult(robot_id,result);

    
    //发送固定路径
    moying_proto::PathPlanDataNotify body;
    body.set_robot_id(robot_id);
    body.set_nav_type(kNavTypeConstantPath);
    *body.mutable_const_path() = cpi;
    publisher_->publish(tm_notify_ids[0], body,false);
    RET_SUCCESS(res);
    
}

void DispatchServer::handlePathPlanService(const moying_proto::PathPlanService_Request& req, moying_proto::PathPlanService_Response& res)
{
    HIGHLIGHT("handlePathPlanService");
    if(!moyingmap_->init_success()){
        RET_FAIL(res,kReqInternalErr,"moying map is not initialized correctly");
        return;
    }

    IdType robot_id = req.robot_id();
    navigation::VehiclePtr vehicle = moyingmap_->getVehicle(robot_id);
    
    if(vehicle == nullptr)
    {
        ERROR("Robot ID "<<robot_id<<" does not exist");
        return;
    }

    if(req.nav_type() == NavType::kNavTypeConstantPath){
        navConstantPath(req,res);
        return;
    }

    std::string map_name = req.map_name();
    navigation::VehicleState start_state, goal_state;
    bool ret = true;
    vehicle->getPose(start_state);
    goal_state.x =  req.dest_pt().x();
    goal_state.y =  req.dest_pt().y();
    goal_state.th =  req.dest_pt().theta();

    goal_state.vel_x = 0;
    goal_state.vel_y = 0;
    goal_state.vel_th = 0;

    HIGHLIGHT("Path Search Request-- " << "robot id:" << robot_id << "  " << start_state.x << " " << start_state.y << " " << start_state.th<<" to " <<goal_state.x << " " << goal_state.y << " " << goal_state.th);
    {
#if defined(__DEBUG__)
        std::lock_guard<std::recursive_mutex> lock(vehicle_mutex_);
        if(vehicle_viz_[robot_id]){
            vehicle_viz_[robot_id]->publishPose(goal_state.x, goal_state.y, goal_state.th);
        }
#endif
    }

    if(fabs(goal_state.x - start_state.x) + fabs(goal_state.y - start_state.y) < 0.02 && fabs(goal_state.th - start_state.th) < 0.02)
    {
        HIGHLIGHT("Start point is same as goal point!");
        RET_SUCCESS(res);
        std::vector<PathPoint> path;

        notifyPlanResult(robot_id, 1);
        PathPoint p;
        goal_state.vel_th = goal_state.vel_x = goal_state.vel_y = 0;
        goal_state.t = std::chrono::system_clock::now();
        stateToPathPoint(start_state, p,0,2);
        p.set_move_mode(1); //原地旋转
        path.push_back(p);
        // moyingmap_->unregisterStaticRobotOccupation(vehicle);
        // notifyFollowingPath(path,robot_id);
        notifyPathPlanData(path, robot_id);
        return;
    }

    vehicle->set_nav_mode_info(NavModeInfo(req.nav_mode(),req.nav_wait_timeout()));
    PathPlanningRequest path_plan_req = PathPlanningRequest(vehicle, start_state, goal_state);
    path_plan_req.is_query_from_outside = true;
    path_plan_req.occupy_time_after_arrive = req.occupy_seconds();
    vehicle->set_occupy_time_after_arrive(req.occupy_seconds());
    if(req.path_plan_delay_seconds() > 0)
        path_plan_req.path_plan_start_time = time(0) + req.path_plan_delay_seconds();
    ret = addPathPlanRequest(robot_id,path_plan_req);
    if(ret){
        RET_SUCCESS(res);
    }else{
        RET_INTERNAL_FAIL(res);
    }

    return;
}

void DispatchServer::handleCancelNavPathService(const moying_proto::CancelNavService_Request& req,moying_proto::CancelNavService_Response& res){

    IdType robot_id =  req.robot_id();
    if(moyingmap_ == nullptr)
    {
        RET_INTERNAL_FAIL(res);
        return;
    }

    const auto& vehicles = moyingmap_->vehicles();
    if(vehicles .find(robot_id) == vehicles.end() ){
        res.mutable_result()->set_code(ReqErrType::kReqParamErr); \
        res.mutable_result()->set_msg("robot is not exist!");
        return;
    }

    if(vehicle_monitors_.find(robot_id) != vehicle_monitors_.end()){
        if( vehicle_monitors_[robot_id]->cancelNav() ){
            RET_SUCCESS(res);
            return;
        }
    }

    RET_INTERNAL_FAIL(res);

}

bool DispatchServer::addPathPlanRequest(unsigned int robot_id,const PathPlanningRequest& req){

    if(!moyingmap_->init_success())
        return false;

    std::lock_guard<std::mutex> lock(planning_queue_mutex_);
    for(auto it=planning_queue_.begin();it!=planning_queue_.end();it++){
        if(it->first == robot_id){
            if(it->second.is_global_goal && it->second.is_query_from_outside){  //如果机器人在队列里有来自交通管理模块外部发送的全局路径请求,清除
                planning_queue_.erase(it);
            }
            break;
        }
    }
    planning_queue_.emplace_back(std::make_pair(robot_id,req));
    INFO("add new path plan request! goal_x:"<<req.goal.x<<" goal_y:"<<req.goal.y<<" goal_th:"<<req.goal.th<<" robot_id:"<<robot_id);

    //设置收到路径规划请求标志
    auto vehicles = moyingmap_->vehicles();
    if(vehicles.find(robot_id) != vehicles.end()){
        vehicles[robot_id]->set_recv_path_plan_req(true);
    }

    return true;

}

bool DispatchServer::updateVehiclePathResult(unsigned int robot_id,const PlanningResult& result){

    if(!moyingmap_->init_success())
        return false;

    if(robot_id <= 0)
        return false;
    
    VehiclePtr vehicle = moyingmap_->getVehicle(robot_id);
    if(vehicle == nullptr)
        return false;
    vehicle->set_path_plan_result(result);
    // std::lock_guard<std::recursive_mutex> lock(vehicle_mutex_);
    // if(vehicle_paths_.find(robot_id) != vehicle_paths_.end()){
    //     vehicle_paths_[robot_id].first = result;
    // }else{
    //     vehicle_paths_[robot_id] = std::make_pair<const navigation::PlanningResult&, int>(result,0);
    // }
    return true;
    
}

void DispatchServer::handleAllPathPlanService(const moying_proto::GetAllRobotPathDataService_Request& req, moying_proto::GetAllRobotPathDataService_Response& res)
{
    if(!moyingmap_->init_success()){
        RET_FAIL(res,kReqInternalErr,"moying map is not initialized correctly");
        return;
    }
    if(vehicle_monitors_.empty())
    {
        res.mutable_result()->set_code(ReqErrType::kReqInternalErr);
        res.mutable_result()->set_msg("No robot");
    }
    else
    {
        res.mutable_result()->set_code(ReqErrType::kReqSuccess);
        res.mutable_result()->set_msg("Success");    

        moyingmap_->ergodicRobots([&res,this](VehiclePtr vehicle)->bool{
            if(vehicle == nullptr)
                return false;
            moying_proto::RobotPathData robot_path;
            auto plan_result = vehicle->path_plan_result();
            robot_path.set_nav_type(plan_result.nav_type);
            robot_path.set_robot_id(vehicle->getID());
            if(plan_result.nav_type == kNavTypeConstantPath){
                *robot_path.mutable_const_path() = plan_result.cpi;
            }else{
                std::vector<moying_proto::PathPoint> points;
                getProtoPath(plan_result,points);
                for(auto& point : points)
                {
                    *robot_path.add_pt() = point;
                }
            }
            *res.add_path_data() = robot_path;
            return true;
        });

    }
}

void DispatchServer::handleSwitchMapTMService(const moying_proto::SwitchMapTMService_Request& req,moying_proto::SwitchMapTMService_Response& res){
    
    if(req.workspace().empty() || req.map_name().empty()){
        RET_FAIL(res,moying_proto::kReqParamErr,"request param invalid");
        return;
    }

    bool no_robot_recv_path_plan = true;
    moyingmap_->ergodicRobots([&no_robot_recv_path_plan](VehiclePtr vehicle)->bool{
        if(vehicle->recv_path_plan_req()){
            no_robot_recv_path_plan = false;
            return false;
        }
        return true;
    });

    if(!no_robot_recv_path_plan){
        RET_FAIL(res,moying_proto::kReqInternalErr,"Can't switch map,there are robots to path planning");
        return;
    }

    // switch_map_req_ = req;
    is_updating_map_data_ = true;

    std::thread  switch_map_thread([this,req](void){
        bool ret = initialize(req.workspace(),req.map_name());
        notifyMapSwitchResult(req.workspace(),req.map_name(),ret);
        if(ret){
            SystemManagerCommuClient::instance()->disconnectWithSystemManager();

            {
                std::lock_guard<std::recursive_mutex> lock(vehicle_mutex_);
                vehicle_viz_.clear();
                vehicle_monitors_.clear();
            }

            SystemManagerCommuClient::instance()->subRobotOnOffLineNotify(std::bind(&DispatchServer::robotOnoffLineNotifyCB,this,std::placeholders::_1));
            SystemManagerCommuClient::instance()->subRobotPoseNotify(std::bind(&DispatchServer::robotPoseNotifyCB,this,std::placeholders::_1));

            const ConfigInfo& cfg_info = ScheduleConfig::instance()->getConfigInfo();
            ret = SystemManagerCommuClient::instance()->connectToSystemManager(cfg_info.commu_cfg.system_manage_ip,
                cfg_info.commu_cfg.system_manage_req_port,cfg_info.commu_cfg.system_manage_sub_port,false,"traffic_manager");
            if(ret){
                subSystemManageNotifies();
                LOG_INFO("reconnect system_manager success");
            }else{
                LOG_INFO("reconnect system_manager failed");
            }
        }
        is_updating_map_data_ = false;
    });
    switch_map_thread.detach();
    RET_SUCCESS(res);
}

void DispatchServer::handleOccupyTimeMapService(const moying_proto::OccupyTimeMapService_Request& req,moying_proto::OccupyTimeMapService_Response& res){
    if(!moyingmap_->init_success()){
        RET_FAIL(res,kReqInternalErr,"moying map is not initialized correctly");
        return;
    }
    if( moyingmap_->occupyTimeMap(req.robot_id(),req.occupy_seconds()) ){
        RET_SUCCESS(res);
    }else{
        RET_INTERNAL_FAIL(res);
    }
}

void DispatchServer::handleWhetherPoseInOneWayRoadService(const moying_proto::WhetherPoseInOneWayRoadService_Request& req,
            moying_proto::WhetherPoseInOneWayRoadService_Response& res){

    unsigned int road_id = 0;
    std::vector<unsigned int> vehicle_ids;
    moyingmap_->isPoseInOneWayRoad(req.pose().x(),req.pose().y(),road_id,vehicle_ids);

    res.set_road_id(road_id);
    for(auto id : vehicle_ids){
        res.add_robot_id(id);
    }
    RET_SUCCESS(res);
}


void DispatchServer::notifyMapSwitchResult(std::string ws,std::string map,bool switch_result)
{
    moying_proto::MapSwitchResultTMNotify body;
    body.set_workspace(ws);
    body.set_map_name(map);
    body.set_switch_result(switch_result);
    publisher_->publish(tm_notify_ids[2], body);

}

void DispatchServer::notifyPlanResult(unsigned int robot_id, unsigned int result)
{
    moying_proto::PathPlanResultNotify body;
    body.set_result(result);
    body.set_robot_id(robot_id);
    publisher_->publish(tm_notify_ids[1], body);

}

void DispatchServer::notifyPathPlanData(const std::vector<moying_proto::PathPoint>& points, unsigned int robot_id)
{
    ConfigInfo& cfg_info = ScheduleConfig::instance()->getConfigInfo();

    moying_proto::PathPlanDataNotify body;
    body.set_nav_type(kNavTypeTrafficManager);
    body.set_robot_id(robot_id);
    string map_name = "";
    body.set_map_name(map_name);
    auto vehicle = moyingmap_->getVehicle(robot_id);
    if(vehicle == nullptr){
        WARNING("getVehicle return nullptr");
        return;
    }
    body.set_follow_mode(0);
    body.set_nav_speed(cfg_info.traffic_manager_cfg.robot_nav_speed);
    for(auto& point : points)
    {
        *body.add_pt() = point;
    }
    publisher_->publish(tm_notify_ids[0], body,false);
}

void DispatchServer::notifyRobotCannotArrive(const moying_proto::RobotCannotArriveDestNotify& notify){

    WARNING("robot cannot arrive dest pose ,robot_id:"<<notify.robot_id()<<" x:"<<notify.dest_pose().x()<<" y:"<<notify.dest_pose().y()
        <<" theta:"<<notify.dest_pose().theta());
    publisher_->publish(tm_notify_ids[3],notify);

    //调用导航状态通知处理函数,触发处理
    // NavStateNotify ns_notify;
    // ns_notify.set_robot_id(notify.robot_id());
    // *ns_notify.mutable_state_info()->mutable_pose() = notify.dest_pose();
    // ns_notify.mutable_state_info()->set_state(NavState::kNavFailed);
    // robotNavStateNotifyCB(ns_notify);
}

// void DispatchServer::notifyFollowingPath(const std::vector<moying_proto::PathPoint>& points,IdType robot_id)
// {
//     moying_proto::PathFollowNotify body;
//     body.set_robot_id(robot_id);
//     body.set_map_name("");
//     for(int i=0;i<points.size();i++)
//     {
//         auto point = points[i];
//         *body.add_pt() = point;
//     }
//     publisher_->publish(tm_notify_ids[1], body);
//     INFO("publish nav path, path size: "<<points.size()<<" robot_id:"<<robot_id);
// }



void DispatchServer::showPathPoint(unsigned int id, navigation::PlanningResult result,std::vector<moying_proto::PathPoint>& points)
{
    if(points.size() == 0)
    {
        return;
        ERROR("Path size is 0!");
    }
    else
    {
        for(auto node : result.node_path){
            HIGHLIGHT("robot_id:"<<id<<" x:"<<node.x<<" y:"<<node.y<<" th:"<<(int)node.th<<" sm:"<<(int)node.sm_state<<" t:"<<node.t);
        }

        HIGHLIGHT("------------------");

        auto now  = std::chrono::system_clock::now();
        HIGHLIGHT("now:"<<std::chrono::duration_cast<std::chrono::milliseconds>(now.time_since_epoch()).count());
        for(int i = 0; i < points.size(); i++)
        {
            HIGHLIGHT("Robot_id:" << id <<" x:" << points[i].pose().x() << " y:" << points[i].pose().y() << " th:" << points[i].pose().theta() << 
                    " vx:" << points[i].vx() << " vy:" << points[i].vy() << " move_mode:"<<points[i].move_mode() << 
                    " cell_x:" << result.node_path[i].x << " cell_y:" << result.node_path[i].y << " sm:" << (int)result.node_path[i].sm_state << " t:" << result.node_path[i].t << 
                      " index:" <<  points[i].pt_index() << " arrive_time:"<<points[i].timestamp());
        }
    }
}

void DispatchServer::getProtoPath(const navigation::PlanningResult& result,std::vector<moying_proto::PathPoint>& path)
{
    
    for(int i = 0; i < result.path.size(); i++)
    {

        PathPoint p;
        robotStateToPathPoint(result.path[i], p, i);
        if(i > 0){
            auto interval = std::chrono::duration_cast<std::chrono::milliseconds>(result.path[i].t - result.path[i-1].t).count();
            p.set_interval_time(interval);
            auto n1 = result.node_path[i-1];
            auto n2 = result.node_path[i];
            if(n1.isEqual(n2)){
                p.set_wait_in_place(true);
            }
        }
        if(i == 0)
        {
            p.set_property(0);
            p.set_is_last_pos(false);
        }
        else if(i != result.path.size() - 1)
        {
            p.set_property(1);
            p.set_is_last_pos(false);
        }
        
        if(i == result.path.size() - 1){
            p.set_property(2);
            p.set_is_last_pos(true);
        }
        path.push_back(p);
    }
}

void DispatchServer::robotStateToPathPoint(const VehicleState &state, moying_proto::PathPoint &point, int index)
{
    point.set_vth(0);
    point.mutable_pose()->set_x(state.x);
    point.mutable_pose()->set_y(state.y);
    point.mutable_pose()->set_theta(state.th);
    point.set_timestamp(std::chrono::duration_cast<std::chrono::milliseconds>(state.t.time_since_epoch()).count());
    point.set_pt_index(index);

    point.set_vx(state.vel_x);
    point.set_vy(state.vel_y);
    point.set_move_mode(state.move_mode); 

   
}

bool DispatchServer::getPathResult(unsigned int robot_id, navigation::PlanningResult& path_result)
{
    if(!moyingmap_->init_success()){
        return false;
    }
    auto vehicle = moyingmap_->getVehicle(robot_id);
    if(vehicle == nullptr)
        return false;
    path_result = vehicle->path_plan_result();
    // std::lock_guard<std::recursive_mutex> lock(vehicle_mutex_);
    // if(vehicle_paths_.find(robot_id) == vehicle_paths_.end())
    // {
    //     ERROR("There is no robot id:" << robot_id << "in path result map.");
    //     return false;
    // }

    // path_result = vehicle_paths_[robot_id].first;
    return true;
}

void DispatchServer::delayMs2delays(int delay_ms, int& delay_s)
{
    double time_delay_s = static_cast<double>(delay_ms) / 1000.0;

    if(time_delay_s >= 0)
    {
        delay_s = int(time_delay_s + 0.5);
    }
    else
    {
        delay_s = int(time_delay_s - 0.5);
    }
}

void DispatchServer::showResultTime(unsigned int robot_id)
{
    navigation::PlanningResult result;
    auto vehicle = moyingmap_->getVehicle(robot_id);
    if(vehicle == nullptr)
        return;
    result = vehicle->path_plan_result();
    // {
    //     std::lock_guard<std::recursive_mutex> lock(vehicle_mutex_);
    //     result = vehicle_paths_[robot_id].first;
    // }
    for(int i = 0; i < result.path.size(); ++i)
    {
        int t = std::chrono::duration_cast<std::chrono::milliseconds>(result.path[i].t - result.start_time).count() / 1000;
        std::cout << i << ":" << t << "  "; 
    }
    std::cout << "\n";

    std::map<int, std::vector<FootprintCell>>::iterator it = result.timed_footprints.begin();
    for(; it != result.timed_footprints.end(); ++it)
    {
        std::cout << it->first << "  ";
    }
    std::cout << "\n";

    for(int i = 0; i < result.node_path.size(); ++i)
    {
        std::cout << result.node_path[i].t << "  ";
    }
    std::cout << "\n";

    for(int i = 0; i < result.path.size(); ++i)
    {
        std::cout << result.path[i].th << "  ";
    }
    std::cout << "\n";

}




} // namespace communication
} // namespace moying

